#pragma once
#include <algorithm>

template <std::random_access_iterator Iter>
void heapify(Iter first, Iter last, Iter root) {
	auto len = std::distance(first, last);
	auto rootIndex = std::distance(first, root);

	auto largest = root;
	auto leftIndex = 2 * rootIndex + 1;
	auto rightIndex = 2 * rootIndex + 2;

	Iter left = (leftIndex < len) ? first + leftIndex : last;
	Iter right = (rightIndex < len) ? first + rightIndex : last;

	if (left < last && *left > *largest) {
		largest = left;
	}
	if (right < last && *right > *largest) {
		largest = right;
	}

	if (root != largest) {
		std::iter_swap(largest, root);

		heapify(first, last, largest);
	}
}

template <std::random_access_iterator Iter>
void heapSort(Iter first, Iter last) {
	auto len = std::distance(first, last);

	for (auto i = len / 2 - 1; i >= 0; i--) {
		heapify(first, last, first + i);
	}

	for (auto i = len - 1; i >= 0; i--) {
		std::iter_swap(first, first + i);

		heapify(first, first + i, first);
	}
}